#include <mrpt/poses.h>
#include <Eigen/Core>
//#include <Eigen/Array>
#include <Eigen/Geometry>
#include <iostream>
#include <math.h>


using namespace Eigen;
using namespace std;

int main()
{
    Vector3d pose1(2,1,M_PI/6.0);//2,1,30deg
    Vector3d pose2(3,2,M_PI/3.0);//2,1,45deg
    Rotation2D<double> ro1(pose1.z());

    Rotation2D<double> ro2(pose2.z());
    Vector3d pose12;
    mrpt::poses::CPose2D cpose1(2,1,M_PI/6.0);
    mrpt::poses::CPose2D cpose2(3,2,M_PI/3.0);
    mrpt::poses::CPose2D cpose12=cpose2-cpose1;
    cout<<"mrpt:"<<cpose12.x()<<"  "<<cpose12.y()<<"  "<<cpose12.phi()*180/M_PI<<endl;

//pose point 2 in frame 1;
    Vector3d dltPose=pose2- pose1;
    //pose12.head<2>()=ro1.inverse()*dltPose.head<2>();
    pose12.z()=pose2.z()-pose1.z();
    cout<<pose12<<endl<<atan2(pose12.y(),pose12.x())*180/M_PI<<endl<<pose12.z()*180/M_PI<<endl;
    return 0;
}
